use std::{collections::BTreeMap, sync::Arc, time::Duration};

use futures::{future::FutureExt, pin_mut, select};
use log::{error, info};
use tokio::{
    spawn,
    sync::{self, RwLock},
    task::JoinHandle,
};

use super::carla_proxy;
use super::client_proxy::{self, DownMessage, Sensors, TestResult, UpMessage};
use super::coord_util::*;
use super::core_state;
use super::timer_util::Timer;
use crate::{Error, Result};

pub struct Server {
    _h_tx: sync::oneshot::Sender<()>,
    jh: JoinHandle<()>,
}

impl Server {
    pub fn new(
        client_proxy_reciever: client_proxy::RuntimeReciever,
        mut client_proxy_sender: client_proxy::Sender,
        mut carla_proxy: carla_proxy::RuntimeProxy,
        core_state: Arc<RwLock<core_state::State>>,
    ) -> Self {
        let (h_tx, h_rx) = sync::oneshot::channel();

        let jh = spawn(async move {
            // TODO: 传感器元周期
            let timer = Timer::new(Duration::from_millis(10));
            let timer_fut = timer.step().fuse();
            let msg_fut = client_proxy_reciever.recv().fuse();
            let stop_fut = h_rx.fuse();
            pin_mut!(timer_fut, msg_fut, stop_fut);

            loop {
                select! {
                    _ = timer_fut => {
                        if let Ok(snapshot) = carla_proxy.get_snapshot().await {
                            let msgs = {
                                let mut core_state = core_state.write().await;

                                // 更新运行时间
                                let running_time = snapshot.running_time;
                                core_state.set_running_time(running_time);

                                // 场景运行结果
                                let test_result = core_state.get_scenario_result().map(|success| {
                                    TestResult {
                                        success,
                                        // TODO:
                                        criteria: BTreeMap::new(),
                                    }
                                });

                                let obstacles = snapshot.obstacles;

                                snapshot
                                    .vehicle_sensors
                                    .into_iter()
                                    .filter_map(|(vehicle_id, carla_proxy::Sensors {
                                        localization,
                                        chassis,
                                        traffic_light,
                                        collision_event,
                                        lane_invasion_event,
                                        camera_images
                                    })| {
                                        core_state
                                            .get_player_with_vehicle_id(vehicle_id.as_str())
                                            .and_then(|conn_id| {
                                                core_state
                                                    .update_player_sensors_time(&conn_id, running_time)
                                                    .and_then(|(update_gnss, update_chassis, update_od, sensors_config)| {
                                                        let localization =
                                                            localization.and_then(|loc| {
                                                                if update_gnss {
                                                                    Some(client_proxy::LocalizationInfo {
                                                                        timestamp: c2n_timestamp(loc.base.get_timestamp()),
                                                                        cartesian_lla: loc.base.get_lla(),
                                                                        position_xyz: c2n_location(loc.base.get_location()),
                                                                        orientation_wxyz: c2n_rotation(loc.base.get_rotation()),
                                                                        linear_vel_xyz: c2n_vector(loc.velocity),
                                                                        linear_accel_xyz: c2n_vector(loc.acceleration),
                                                                        angular_vel_xyz: c2n_angles(loc.angular_velocity),
                                                                    })
                                                                } else {
                                                                    None
                                                                }
                                                            });

                                                        let perception = if update_od {
                                                            obstacles.get(&vehicle_id).map(|ego| {
                                                                let tf = Transform::from_base_point(
                                                                    c2n_location(ego.location_xyz),
                                                                    c2n_rotation(ego.rotation_pyr),
                                                                ).inverse();
                                                                let (p, o) = tf.transform(
                                                                    sensors_config.gnss.position_xyz,
                                                                    sensors_config.gnss.orientation_wxyz);
                                                                Transform::from_base_point(p, o)
                                                            }).and_then(|ego_tf| {
                                                                obstacles.get(&vehicle_id).map(|_| {
                                                                    obstacles.iter().filter(|(id, _)| {
                                                                        &&vehicle_id != id
                                                                    }).fold(Vec::new(), |mut v, (id, obstacle)| {
                                                                        // 转换到gnss传感器相对坐标系
                                                                        let (position_xyz, orientation_wxyz) =
                                                                            ego_tf.transform(c2n_location(obstacle.location_xyz),
                                                                            c2n_rotation(obstacle.rotation_pyr));
                                                                        let linear_vel_xyz =
                                                                            ego_tf.transform_vector(c2n_vector(obstacle.linear_vel_xyz));

                                                                        let ob_info = client_proxy::ObstacleInfo {
                                                                            id: id.clone(),
                                                                            o_type: {
                                                                                match obstacle.type_tag {
                                                                                    4 => client_proxy::ObstacleType::Pedestrian,
                                                                                    10 => client_proxy::ObstacleType::Vehicle,
                                                                                    _ => client_proxy::ObstacleType::Unknow,
                                                                                }
                                                                            },
                                                                            is_static: obstacle.is_static,
                                                                            bounding_box: client_proxy::BoundingBox {
                                                                                position_xyz:
                                                                                    c2n_location(obstacle.bounding_box.location_xyz),
                                                                                orientation_wxyz:
                                                                                    c2n_rotation(obstacle.bounding_box.rotation_pyr),
                                                                                extent_xyz: (
                                                                                    obstacle.bounding_box.extent_xyz.0 as f64,
                                                                                    obstacle.bounding_box.extent_xyz.1 as f64,
                                                                                    obstacle.bounding_box.extent_xyz.2 as f64),
                                                                            },
                                                                            position_xyz,
                                                                            orientation_wxyz,
                                                                            linear_vel_xyz,
                                                                        };

                                                                        v.push(ob_info);
                                                                        v
                                                                    })
                                                                }).map(|obstacles| {
                                                                    let traffic_light = traffic_light.map(|tl| {
                                                                        client_proxy::TrafficLightInfo {
                                                                            timestamp: tl.timestamp,
                                                                            position_xyz: c2n_location(tl.location_xyz),
                                                                            orientation_wxyz: c2n_rotation(tl.rotation_pyr),
                                                                            state: match tl.state {
                                                                                0 => client_proxy::TrafficLightState::Red,
                                                                                1 => client_proxy::TrafficLightState::Yellow,
                                                                                2 => client_proxy::TrafficLightState::Green,
                                                                                3 => client_proxy::TrafficLightState::Off,
                                                                                _=> client_proxy::TrafficLightState::Unknown,
                                                                            }
                                                                        }
                                                                    });

                                                                    client_proxy::PerceptionInfo {
                                                                        timestamp: running_time,
                                                                        obstacles,
                                                                        traffic_light,
                                                                    }
                                                                })
                                                            })
                                                        } else {
                                                            None
                                                        };

                                                        let chassis = chassis.and_then(
                                                            |chassis| {
                                                                if update_chassis {
                                                                    Some(client_proxy::ChassisInfo {
                                                                        timestamp: chassis.timestamp,
                                                                        control: client_proxy::VehicleControl {
                                                                            throttle: chassis.control.throttle as f64,
                                                                            // 需要反转
                                                                            steer: -chassis.control.steer as f64,
                                                                            brake: chassis.control.brake as f64,
                                                                            hand_brake: chassis.control.hand_brake,
                                                                            reverse: chassis.control.reverse,
                                                                            manual_gear_shift: chassis.control.manual_gear_shift,
                                                                            gear: chassis.control.gear,
                                                                        }
                                                                    })
                                                                } else {
                                                                    None
                                                                }
                                                        });

                                                        let collision_event = collision_event.map(|event|
                                                            client_proxy::CollisionEvent {
                                                                timestamp: event.get_timestamp(),
                                                                normal_impulse: c2n_vector(event.get_normal_impulse()),
                                                            });

                                                        let lane_invasion_event = lane_invasion_event.map(|event|
                                                            client_proxy::LaneInvasionEvent {
                                                                timestamp: event.get_timestamp(),
                                                                lane_change: event.get_lane_change().iter().map(|v|
                                                                    match v {
                                                                        1 => client_proxy::LaneChangeType::Right,
                                                                        2 => client_proxy::LaneChangeType::Left,
                                                                        3 => client_proxy::LaneChangeType::Both,
                                                                        _ => client_proxy::LaneChangeType::None,
                                                                    }).collect(),
                                                            });

                                                        let camera_images = camera_images.into_iter()
                                                            .map(|(camera_id, image)| {
                                                            let is_gray = sensors_config.cameras.get(&camera_id)
                                                                .map_or(true, |config| config.is_gray);

                                                            let raw = image.get_bgra_pixels();
                                                            let data = if is_gray {
                                                                let mut data = vec![0u8; (raw.len() / 4)
                                                                    as usize];
                                                                for i in 0..data.len() {
                                                                    data[i] = ((raw[i*4] as u16 + raw[i*4+1] as u16 + raw[i*4+2] as u16)/3) as u8;
                                                                }

                                                                data
                                                            } else {
                                                                let mut data = vec![0u8; (raw.len() / 4 * 3)
                                                                    as usize];
                                                                for i in 0..data.len() / 3 {
                                                                    data[i*3] = raw[i*4];
                                                                    data[i*3+1] = raw[i*4+1];
                                                                    data[i*3+2] = raw[i*4+2];
                                                                }

                                                                data
                                                            };

                                                            let mut image = client_proxy::Image {
                                                                timestamp: image.get_timestamp(),
                                                                camera_id,
                                                                width: image.get_width(),
                                                                height: image.get_height(),
                                                                is_gray,
                                                                data,
                                                            };
                                                            image.compress_data();
                                                            image
                                                        }).collect::<Vec<_>>();

                                                        if test_result.is_some() || localization.is_some() || perception.is_some()
                                                            || chassis.is_some() || collision_event.is_some() || lane_invasion_event.is_some()
                                                            || !camera_images.is_empty() {
                                                            Some((
                                                                conn_id,
                                                                DownMessage::Sensors(Sensors {
                                                                    test_result: test_result.clone(),
                                                                    localization,
                                                                    perception,
                                                                    chassis,
                                                                    collision_event,
                                                                    lane_invasion_event,
                                                                    camera_images,
                                                                }),
                                                            ))
                                                        } else {
                                                            None
                                                        }
                                                    })
                                            })
                                    })
                                    .collect::<Vec<_>>()
                            };

                            for (conn_id, msg) in msgs {
                                if let Err(e) = client_proxy_sender.send(conn_id, msg).await {
                                    error!("{}", e);
                                }
                            }
                        } else {
                            error!("Carla proxy error");
                            break;
                        }

                        timer_fut.set(timer.step().fuse());
                    }

                    msg = msg_fut => {
                        match msg {
                            Ok((rx, conn_id, msg)) => {
                                match msg {
                                    UpMessage::VehicleControl(control) => {
                                        let core_state = core_state.read().await;

                                        if let Some((_, _, vehicle_id, _)) = core_state.get_player_info(&conn_id) {
                                            if let Err(e) = carla_proxy
                                                .apply_vehicle_control(
                                                    vehicle_id.as_str(),
                                                    &carla_proxy::VehicleControl {
                                                        throttle: control.throttle as f32,
                                                        // 需要反转
                                                        steer: -control.steer as f32,
                                                        brake: control.brake as f32,
                                                        hand_brake: control.hand_brake,
                                                        reverse: control.reverse,
                                                        manual_gear_shift: control.manual_gear_shift,
                                                        gear: control.gear,
                                                    },
                                                )
                                                .await {
                                                error!("{}", e);
                                            }
                                        }
                                    }

                                    _=> ()
                                }

                                msg_fut.set(rx.recv().fuse());
                            }

                            Err(e) => {
                                error!("{}", e);
                                break;
                            }
                        }
                    }

                    _ = stop_fut => {
                        info!("Stop runtime server");
                        break;
                    }
                }
            }
        });

        Self { _h_tx: h_tx, jh }
    }

    pub async fn wait(self) -> Result<()> {
        self.jh.await.map_err(Error::from)
    }
}
